import rosbag
import rospy
import csv
from sensor_msgs.msg import Imu

bag = rosbag.Bag('imu.bag', 'w')

with open('imu0.csv', 'r') as csvfile:
    reader = csv.reader(csvfile)
    next(reader) # skip header row
    for row in reader:
        imu_msg = Imu()
        imu_msg.header.stamp = rospy.Time.from_sec(float(row[0])*1e-9)
        imu_msg.angular_velocity.x = float(row[1])
        imu_msg.angular_velocity.y = float(row[2])
        imu_msg.angular_velocity.z = float(row[3])
        imu_msg.linear_acceleration.x = float(row[4])
        imu_msg.linear_acceleration.y = float(row[5])
        imu_msg.linear_acceleration.z = float(row[6])
        bag.write('/imu', imu_msg, imu_msg.header.stamp)

bag.close()
